from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    # 声明参数
    uav_namespace_arg = DeclareLaunchArgument(
        'uav_namespace',
        default_value='uav_swarms',
        description='Namespace for UAV swarms.'
    )

    vehicle_type_arg = DeclareLaunchArgument(
        'vehicle_type',
        default_value='drone',
        description='Type of vehicle.'
    )

    vehicle_num_arg = DeclareLaunchArgument(
        'vehicle_num',
        default_value="16",
        description='Number of vehicles.'
    )

    safe_distance_arg = DeclareLaunchArgument(
        'safe_distance',
        default_value="5.0",  # 仍然是字符串形式，但会在节点中进行类型转换
        description='Safe distance of vehicles.'
    )

    # 调试日志，显示参数值
    log_uav_namespace = LogInfo(msg=["[DEBUG] UAV Namespace: ", LaunchConfiguration("uav_namespace")])
    log_vehicle_type = LogInfo(msg=["[DEBUG] Vehicle Type: ", LaunchConfiguration("vehicle_type")])
    log_vehicle_num = LogInfo(msg=["[DEBUG] Vehicle Num: ", LaunchConfiguration("vehicle_num")])
    log_safe_distance = LogInfo(msg=["[DEBUG] Safe Distance: ", LaunchConfiguration("safe_distance")])

    # 创建节点
    communication_multirotor_node = Node(
        package="uav_swarms",
        executable="avoid_node",
        name="avoid_node",
        output="screen",
        emulate_tty=True,
        parameters=[
            {"uav_namespace": LaunchConfiguration('uav_namespace')},
            {"vehicle_num": LaunchConfiguration('vehicle_num')},
            {"vehicle_type": LaunchConfiguration('vehicle_type')},
            {"safe_distance": LaunchConfiguration('safe_distance')},
        ]
    )

    # 返回 LaunchDescription，包含参数和节点
    return LaunchDescription([
        uav_namespace_arg,
        vehicle_type_arg,
        vehicle_num_arg,
        safe_distance_arg,
        log_uav_namespace,
        log_vehicle_type,
        log_vehicle_num,
        log_safe_distance,
        communication_multirotor_node
    ])
